
#include <ddk.h>
#include <elapi.h>
#include <display.h>
#include <stdio.h>

typedef struct tagBitmapFileHeader {
	Int32 bfSize;
	Int16 bfReserved1;
	Int16 bfReserved2;
	Int32 bfOffBits;
} BitmapFileHeader;

typedef struct tagBitmapInfoHeader {
	Int32 biSize;
	Int32 biWidth;
	Int32 biHeight;
	Int16 biPlanes;
	Int16 biBitCount;
	Int32 biCompression;
	Int32 biSizeImage;
	Int32 biXPelsPerMeter;
	Int32 biYPelsPerMeter;
	Int32 biClrUsed;
	Int32 biClrImportant;
} BitmapInfoHeader;

static Void WriteBmpHeader(
	FILE * fp,
	ColorFormat uColorFormat,
	Int32 nBpp,
	Int32 nWidth,
	Int32 nHeight)
{
	BitmapFileHeader bitmapFileHeader = { 0 };
	BitmapInfoHeader bitmapInfoHeader = { 0 };

	bitmapFileHeader.bfSize = 2 + sizeof(BitmapFileHeader)
		+ sizeof(BitmapInfoHeader) + ((nBpp * nWidth * nHeight) >> 3);
	bitmapFileHeader.bfOffBits = 2 + sizeof(BitmapFileHeader)
		+ sizeof(BitmapInfoHeader);

	bitmapInfoHeader.biSize = sizeof(BitmapInfoHeader);
	bitmapInfoHeader.biWidth = nWidth;
	bitmapInfoHeader.biHeight = nHeight;
	bitmapInfoHeader.biPlanes = 1;
	bitmapInfoHeader.biBitCount = nBpp;
	bitmapInfoHeader.biSizeImage = (nBpp * nWidth * nHeight) >> 3;

	if (nBpp == 16 || nBpp == 32) {
		bitmapFileHeader.bfSize += 12;
		bitmapFileHeader.bfOffBits += 12;
		bitmapInfoHeader.biCompression = 3;
	}

	fwrite("BM", 2, 1, fp);
	fwrite(&bitmapFileHeader, sizeof(BitmapFileHeader), 1, fp);
    fwrite(&bitmapInfoHeader, sizeof(BitmapInfoHeader), 1, fp);

	Int32 rgbQuad[3];

	if (nBpp == 16) {
		if (uColorFormat == COLOR_RGB565) {
	    	rgbQuad[0] = 0x0000F800;
	    	rgbQuad[1] = 0x000007E0;
	    	rgbQuad[2] = 0x0000001F;
	    	fwrite(rgbQuad, sizeof(rgbQuad), 1, fp);
		}
	}
	else if (nBpp == 32) {
		if (uColorFormat == COLOR_RGBT666) {
	    	rgbQuad[0] = 0x0003F000;
	    	rgbQuad[1] = 0x00000FC0;
	    	rgbQuad[2] = 0x0000003F;
	    	fwrite(rgbQuad, sizeof(rgbQuad), 1, fp);
	    }
	}
}

EXTERN_C ECode CaptureFrame(WChar *pwszPath)
{
	ECode ec = NOERROR;

	FILE *fpBitmap = _wfopen(pwszPath, L"wb");
	if (!fpBitmap) return E_FS_IO_ERROR;

	IDeviceDriver *pIBaseplane = NULL;
	MemoryBuf_< 32 > mbAddress;
	MemoryBuf_< sizeof(FrameBufferFormat) > mbFormat;

	ec = _CSystem_FindRunningObject(L"device:display0",
		(PInterface *)&pIBaseplane);
	if (FAILED(ec)) return ec;

    ec = pIBaseplane->Control(3, NULL_MEMORYBUF, &mbAddress, NULL);
    if (FAILED(ec)) {
    	pIBaseplane->Release();
    	return ec;
    }

    Address *pFrameBuffer = (Address *)*(Address *)mbAddress.GetPayload();

    ec = pIBaseplane->Control(7, NULL_MEMORYBUF, &mbFormat, NULL);
    if (FAILED(ec)) {
    	pIBaseplane->Release();
    	return ec;
    }

    FrameBufferFormat *pFormat = (FrameBufferFormat *)mbFormat.GetPayload();

	Int32 nPitch = (pFormat->Width * pFormat->Bpp) >> 3;
	Int32 nFrameSize = (pFormat->Bpp * pFormat->Width * pFormat->Height) >> 3;
	Int32 nHeight = pFormat->Height;

	Byte *pbCaptureBuffer = (Byte *)malloc(nFrameSize);
	if (!pbCaptureBuffer) {
		pIBaseplane->Release();
		return E_OUT_OF_MEMORY;
	}

	for (Int32 nIndex = nHeight - 1; nIndex >= 0; nIndex--) {
		memcpy(
			(Byte *)pbCaptureBuffer + nIndex * nPitch,
			(Byte *)pFrameBuffer + (nHeight - nIndex - 1) * nPitch,
			nPitch
		);
	}

	WriteBmpHeader(fpBitmap, pFormat->Color,
		pFormat->Bpp, pFormat->Width, pFormat->Height);

	fwrite(pbCaptureBuffer, nFrameSize, 1, fpBitmap);
	fclose(fpBitmap);

	pIBaseplane->Release();
	free(pbCaptureBuffer);

	return ec;
}
